#include "data_reflow/data_catcher.h"

namespace data_reflow
{
    SensorDataPub::SensorDataPub() : Node("test_data_catcher_node")
    {
        load_config();
        // 创建发布者
        pub_camera_ = this->create_publisher<SensorData>("camera_data_pub", 10);
        pub_lidar_ = this->create_publisher<SensorData>("lidar_data_pub", 10);

        // 启动摄像头采集（发布+存储）
        if (camera_record_)
        {
            auto period = std::chrono::milliseconds(1000 / camera_frequency_);
            camera_timer_ = this->create_wall_timer(
                period, std::bind(&SensorDataPub::publish_camera_data, this));
            RCLCPP_INFO(this->get_logger(), "摄像头数据采集已启动，频率：%d Hz（发布+存储）", camera_frequency_);
        }
        else
        {
            RCLCPP_WARN(this->get_logger(), "摄像头录制未开启");
        }

        // 启动激光雷达采集（可选，仅发布不存储）
        if (lidar_record_)
        {
            auto period = std::chrono::milliseconds(1000 / lidar_frequency_);
            lidar_timer_ = this->create_wall_timer(
                period, std::bind(&SensorDataPub::publish_lidar_data, this));
            RCLCPP_INFO(this->get_logger(), "激光雷达数据采集已启动，频率：%d Hz", lidar_frequency_);
        }
        else
        {
            RCLCPP_WARN(this->get_logger(), "激光雷达录制未开启");
        }

        RCLCPP_INFO(this->get_logger(), "配置文件加载成功");
    }

    void SensorDataPub::load_config()
    {
        auto& config = ConfigManager::get_instance();
        if (!config.is_loaded())
        {
            RCLCPP_ERROR(this->get_logger(), "配置文件未加载，使用默认参数");
            return;
        }

        camera_frequency_ = config.get_int("publish_frequency_camera", 30);
        lidar_frequency_ = config.get_int("publish_frequency_lidar", 15);
        camera_record_ = config.get_bool("record_camera", true);
        lidar_record_ = config.get_bool("record_lidar", true);

        if (camera_frequency_ <= 0)
        {
            RCLCPP_WARN(this->get_logger(), "摄像头频率无效，使用默认值 30Hz");
            camera_frequency_ = 30;
        }
        if (lidar_frequency_ <= 0)
        {
            RCLCPP_WARN(this->get_logger(), "激光雷达频率无效，使用默认值 15Hz");
            lidar_frequency_ = 15;
        }
    }

    void SensorDataPub::publish_camera_data()
    {
        // 生成摄像头数据
        auto msg = SensorData();
        msg.type = SensorData::TYPE_CAMERA;
        msg.count = camera_count_++;
        msg.stamp = this->get_clock()->now();
        msg.camera_data = "RGB_" + std::to_string(msg.count);

        // 发布数据
        pub_camera_->publish(msg);
        has_camera_published_ = true;

        // 存储数据到单例缓存
        DataBuffer::get_instance().addData(msg);
    }

    void SensorDataPub::publish_lidar_data()
    {
        auto msg = SensorData();
        msg.type = SensorData::TYPE_LIDAR;
        msg.count = lidar_count_++;
        msg.stamp = this->get_clock()->now();
        msg.lidar_data = {
            "PointCloud_" + std::to_string(msg.count) + "_x",
            "PointCloud_" + std::to_string(msg.count) + "_y",
            "PointCloud_" + std::to_string(msg.count) + "_z"
        };

        pub_lidar_->publish(msg);
        has_lidar_published_ = true;
    }

    bool SensorDataPub::isDataCollectedSuccessfully(SensorType sensor_type)
    {
        switch (sensor_type)
        {
            case SensorType::TYPE_CAMERA:
                if (!camera_record_) return false;
                if (!pub_camera_) return false;
                if (!camera_timer_ || !camera_timer_->is_ready()) return false;
                if (!has_camera_published_) return false;
                RCLCPP_DEBUG(this->get_logger(), "摄像头采集校验成功：已发布%u帧", camera_count_);
                return true;

            case SensorType::TYPE_LIDAR:
                if (!lidar_record_) return false;
                if (!pub_lidar_) return false;
                if (!lidar_timer_ || !lidar_timer_->is_ready()) return false;
                if (!has_lidar_published_) return false;
                RCLCPP_DEBUG(this->get_logger(), "激光雷达采集校验成功：已发布%u帧", lidar_count_);
                return true;

            default:
                RCLCPP_WARN(this->get_logger(), "未知传感器类型，校验失败");
                return false;
        }
    }
}  // namespace data_reflow